/* Team 151 - Tough Techs */
/* 2012 - Rebound Rumble - "Hey Jude" */
package edu.wpi.first.wpilibj.templates;

import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import java.util.Timer;
import java.util.TimerTask;

public class War extends SimpleRobot {

    Joystick DRIVER=new Joystick(1),SHOOTER=new Joystick(2);
    CANJaguar fr,fl,br,bl,shooterY,shoot1,shoot2,pickup;
    AnalogChannel lazyPotX,lazyPotY;
    Encoder left,right;
    Famine drive=new Famine(DRIVER,SHOOTER);
    Servo serv=new Servo(2),shooterX=new Servo(1);
    BloodyBacon dash;
    Gyro gyro = new Gyro(1);
    public void robotInit(){
        /*try{
            dash=new BloodyBacon(this);
        }catch(Exception e){System.out.println("War: Cannot Connect To Dashboard");}*/
        System.out.println("War: Initializing CANJaguars");
        try{fr=new CANJaguar(Death.JAGUAR_RIGHT_FRONT);}catch(CANTimeoutException e){System.out.println("War: Initializing 'Front Right' Failed");}
        try{fl=new CANJaguar(Death.JAGUAR_LEFT_FRONT);}catch(CANTimeoutException e){System.out.println("War: Initializing 'Front Left' Failed");}
        try{br=new CANJaguar(Death.JAGUAR_RIGHT_REAR);}catch(CANTimeoutException e){System.out.println("War: Initializing 'Back Right' Failed");}
        try{bl=new CANJaguar(Death.JAGUAR_LEFT_REAR);}catch(CANTimeoutException e){System.out.println("War: Initializing 'Back Left' Failed");}
        try{shooterY=new CANJaguar(Death.JAGUAR_SHOOTER_Y);}catch(CANTimeoutException e){System.out.println("War: Initializing 'Shooter Y' Failed");}
        try{shoot1=new CANJaguar(Death.JAGUAR_SHOOTER_TOP);shoot1.changeControlMode(CANJaguar.ControlMode.kVoltage);}catch(CANTimeoutException e){System.out.println("War: Initializing 'Top Shooter' Failed");}
        try{shoot2=new CANJaguar(Death.JAGUAR_SHOOTER_BOTTOM);shoot2.changeControlMode(CANJaguar.ControlMode.kVoltage);}catch(CANTimeoutException e){System.out.println("War: Initializing 'Bottom Shooter' Failed");}
        try{pickup=new CANJaguar(Death.JAGUAR_PICKUP_POWER);}catch(CANTimeoutException e){System.out.println("War: Initializing 'Pickup' Failed");}
        //balls = Integer.parseInt(dash.requestInfo("BALLS"));
        /*new Timer().schedule(new TimerTask(){
                public void run() {              
                    if (fl!=null) updateCAN(Death.JAGUAR_LEFT_FRONT,"Front Left",fl);
                    if (fr!=null) updateCAN(Death.JAGUAR_RIGHT_FRONT,"Front Right",fr);
                    if (bl!=null) updateCAN(Death.JAGUAR_LEFT_REAR,"Back Left",bl);
                    if (br!=null) updateCAN(Death.JAGUAR_RIGHT_REAR,"Back Right",br);
                    if (shoot1!=null) updateCAN(Death.JAGUAR_SHOOTER_TOP,"Shooter",shoot1);
                    if (shooterY!=null) updateCAN(Death.JAGUAR_SHOOTER_Y,"Shooter Pitch",shooterY);
                    if (pickup!=null) updateCAN(Death.JAGUAR_PICKUP_POWER,"Pickup",pickup);
                    try {dash.updatePWM("Input",Death.GYRO,"Gyro",format(gyro.getAngle(),2));} catch (Exception e){System.out.println("Gyro Failed To Update");}
                    try {dash.updatePWM("Input",Death.POT_X,"Shooter Yaw",format(lazyPotX.getAverageVoltage(),2));} catch (Exception e){System.out.println("Shooter Yaw Failed To Update");}
                    try {dash.updatePWM("Input",Death.POT_Y,"Shooter Pitch",format(lazyPotY.getAverageVoltage(),2));} catch (Exception e){System.out.println("Shooter Pitch Failed To Update");}
                    try {dash.updateInfo("Servo Yaw",format(shooterX.getSpeed(),2)); } catch (Exception e){System.out.println("ShooterX Failed To Update");}
                    try {dash.updateInfo("Servo Dropper",format(serv.getSpeed(),2));} catch (Exception e){System.out.println("Serv Failed To Update");}
                }
                public String format(double num, double dec){
                    return ""+((int)num*(dec=MathUtils.pow(10,dec)))/dec;
                }
                public void updateCAN(int i, String name, CANJaguar jag){
                    try {
                        dash.updateCAN(i, name, format(jag.getX(),2)+"%", format(jag.getBusVoltage(),2)+"V", format(jag.getOutputVoltage(),2)+"V");
                    } catch (CANTimeoutException e) {
                        dash.updateCAN(i, name, "CAN TIMEOUT", "CAN TIMEOUT", "CAN TIMEOUT");
                    } catch (Exception e) {
                        System.out.println("Wars: Cannot Update CAN Jaguar '"+name+"'");
                    }
                }
            },1,100);*/
    }
    public void autonomous() {
        /*while (isAutonomous()) {
            try {
                drive.autoAim(shooterX,shooterY,shoot1,dash,lazyPotX,lazyPotY);
            } catch (CANTimeoutException ex) {
                System.out.println("CAN Timeout: AutoAim (Autonomous)");
            } catch (Exception ex) {
                System.out.println("Exception: AutoAim (Autonomous)");
            }
        }*/
    }
    public void operatorControl(){
        double drivePower=0.5,shootPower=.25;
        int direction=1;
        System.out.println("War: TeleOp");
        while(isEnabled()&&isOperatorControl()){
            drivePower=0.5;
            shootPower=0.5;
            if(DRIVER.getRawButton(Death.DRIVER_RIGHT_BUMPER))drivePower=1;
            if(DRIVER.getRawButton(Death.DRIVER_RIGHT_TRIGGER))drivePower=0.25;
            if(DRIVER.getRawButton(Death.DRIVER_BUTTON_X))direction=1;
            if(DRIVER.getRawButton(Death.DRIVER_BUTTON_Y))direction=-1;
            try{drive.tankDrive(drivePower,direction,fl,fr,bl,br);}catch(CANTimeoutException e){System.out.println("CANJaguars Timed Out: Drive");}
            try{drive.tankPickup(pickup);}catch(Exception e){System.out.println("CANJaguars Timed Out: Pickup");}
            try{drive.tankBridgeDrop(serv);}catch(Exception e){System.out.println("Servo Error: Bridge Drop");}
            try{drive.tankShooter(shootPower,shooterX,shooterY,shoot1,shoot2,pickup);}catch(Exception e){System.out.println("CANJaguars Timed Out: Shooter");}
            try{if(SHOOTER.getRawButton(Death.SHOOTER_BUTTON_A))drive.autoAim(shooterX,shooterY,shoot1,dash,lazyPotX,lazyPotY);}catch(Exception e){System.out.println("CANJaguars Timed Out: Auto Aim");}
        }
    }
}